#include "StdAfx.h"
#include "Actor.h"
#define THRESHOLD  0.1f

namespace banknamespace
{

	Actor::Actor(void)
	{
		m_position = vec3f( 0.0f, 0.0f, 0.0f );
		m_velocity = vec3f( 0.0f, 0.0f, 0.0f );
		m_rotation = vec3f( 0.0f, 0.0f, 0.0f );
		m_drag = 0.3f;

		float Tc[] = { 1.0f, 0.0f, 0.0f, 0.0f,
						0.0f, 1.0f, 0.0f, 0.0f,
						0.0f, 0.0f, 1.0f, 0.0f,
						m_position.x, m_position.y, m_position.z, 1.0f};
		matTc = Matrix4f( Tc );

		float angleYaw = m_rotation.x * 3.1416f / 180.0f;
		float Ryc[] = { 1.0f, 0.0f, 0.0f, 0.0f,
					0.0f, cos(angleYaw),  sin(angleYaw), 0.0f,
					0.0f, -sin(angleYaw), cos(angleYaw), 0.0f,
					0.0f, 0.0f, 0.0f, 1.0f};
		matRyc = Matrix4f( Ryc );

		float anglePitch = -m_rotation.y * 3.1416f / 180.0f;
		float Rpc[] = { cos(anglePitch), 0.0f, sin(anglePitch), 0.0f,
					0.0f, 1.0f,  0.0f, 0.0f,
					-sin(anglePitch), 0.0f, cos(anglePitch), 0.0f,
					0.0f, 0.0f, 0.0f, 1.0f};
		matRpc = Matrix4f( Rpc );

		updateMat();
	}


	Actor::~Actor(void)
	{
	}

	const vec3f& Actor::position() const 
	{
		return m_position;
	}
	void Actor::setPosition( const vec3f& position ) 
	{ 
		m_position = position; 
	}

	const vec3f& Actor::rotation() const 
	{
		return m_rotation;
	}
	void Actor::setRotation( const vec3f& rotation ) 
	{ 
		m_rotation = rotation; 
	}
	void Actor::setVelocity( const vec3f& velocity ) 
	{ 
		m_velocity = velocity; 
	}
	vec3f Actor::getVelocity()
	{
		return m_velocity;
	}

	void Actor::rotate( const vec3f& change ) 
	{ 
		m_rotation += change; 

		m_rotation.x = clamp( m_rotation.x, -90.0f, 90.0f );
	}

	void Actor::applyAngularImpulse( const vec3f& change ) 
	{ 
		/*vec4f tmp4f = actorMatrixInv * vec4f(change);

		m_angularVelocity.x += tmp4f[0];
		m_angularVelocity.y += tmp4f[1];
		m_angularVelocity.z += tmp4f[2];*/

		m_angularVelocity += change;
	}

	void Actor::applyImpulse( const vec3f& impulse )
	{
		
		float tmp[4];
		memcpy( tmp, &impulse, sizeof(float) * 4 );
		tmp[3] = 0.0f;

		// apply euler angle
		vec4f tmp4f = actorMatrixInv * vec4f(tmp);

		m_velocity.x += tmp4f[0];
		m_velocity.y += tmp4f[1];
		m_velocity.z += tmp4f[2];

	}

	void Actor::setViewTransformation()
	{
		//Rc*Tc is Camera->World. We need World->Camera (its inverse)
		//Inverse is transpose for orthogonal
		Matrix4f tmpTc( matTc );
		Matrix4f tmpRyc( matRyc );
		Matrix4f tmpRpc( matRpc );
		actorMatrix = tmpRyc * tmpRpc * tmpTc;

		tmpRyc.transpose();
		tmpRpc.transpose();

		//Inverse for translation is negation
		tmpTc.data()[12] = -tmpTc.data()[12];
		tmpTc.data()[13] = -tmpTc.data()[13];
		tmpTc.data()[14] = -tmpTc.data()[14];

		actorMatrixInv = tmpTc * tmpRpc * tmpRyc;
		//glLoadMatrixf( camMatrixInv.data );
	}

	void Actor::updateMat()
	{
		// rotate
		m_angularVelocity -= m_angularVelocity * m_drag;
		m_rotation += m_angularVelocity;
		m_rotation.x = clamp( m_rotation.x, -90.0f, 90.0f );
		float angleYaw = m_rotation.x * 3.1416f / 180.0f;
		float anglePitch = -m_rotation.y * 3.1416f / 180.0f;
		matRyc.data()[5] = cos( angleYaw );
		matRyc.data()[6] = sin( angleYaw );
		matRyc.data()[9] = -sin( angleYaw );
		matRyc.data()[10] = cos( angleYaw );

		matRpc.data()[0] = cos( anglePitch );
		matRpc.data()[2] = sin( anglePitch );
		matRpc.data()[8] = -sin( anglePitch );
		matRpc.data()[10] = cos( anglePitch );

		// translate
		m_velocity -= m_velocity * m_drag;
		m_position += m_velocity;
		matTc.data()[12] = m_position.x;
		matTc.data()[13] = m_position.y;
		matTc.data()[14] = m_position.z;

		// calc euler
		matEulerRotation = matRyc * matRpc;
		setViewTransformation();

		// rigid body force
		//m_angularMomentum -= m_angularMomentum * m_drag;
	}

	void Actor::applyAngularMomentum(const vec3f& M)
	{
		//m_angularMomentum += M;
	}

	void Actor::applyForce(vec3f ri, vec3f forceDir)
	{
		m_AngularMomentum += ri.crossProduct( forceDir ); // if more forces , need to calc the summation
		actorState.m_prev.ForceDir += forceDir;
		trace("force: "<<forceDir.x<<","<<forceDir.y<<","<<forceDir.z<<std::endl);
	}

	void Actor::clearForce()
	{
		m_AngularMomentum =	actorState.m_prev.ForceDir  = vec3f();
	}

	void Actor::updatePhysics(float deltaTime){
		actorState.m_prev.Rcm = m_position;//prev pos

		//calc the next state
		// 	Matrix3f Irb = Matrix3f(2/3.0f,-0.25f,-0.25f,
		// 							-0.25f,2/3.0f,-0.25f,
		// 							-0.25f,-0.25f,2/3.0f) * actorState.m_prev.Mass * 100;//(10*10 cube)
		


		actorState.m_next.I_inv = actorState.m_prev.I_inv = 
			actorState.m_prev.R * (actorState.m_prev.Irb.inverse()) * (actorState.m_prev.R.transpose());
		 
		//trace("Inv:"<<std::endl<<actorState.m_prev.I_inv[0]<<","<<actorState.m_prev.I_inv[1]<<","<<actorState.m_prev.I_inv[2]<<std::endl<<
		//	actorState.m_prev.I_inv[3]<<","<<actorState.m_prev.I_inv[4]<<","<<actorState.m_prev.I_inv[5]<<std::endl<<
		//	actorState.m_prev.I_inv[6]<<","<<actorState.m_prev.I_inv[7]<<","<<actorState.m_prev.I_inv[8]<<std::endl);
		//

 
 
		actorState.m_next.Hi =  actorState.m_prev.Hi + m_AngularMomentum; 
		actorState.m_next.w = actorState.m_prev.I_inv * actorState.m_prev.Hi;//calc new w 

		actorState.m_next.Vcm =  actorState.m_prev.Vcm + actorState.m_prev.ForceDir / (actorState.m_prev.Mass);//  * deltaTime;
		actorState.m_next.Rcm = actorState.m_prev.Rcm + actorState.m_next.Vcm ;//	  * deltaTime;

		// if(actorState.m_next.w !=vec3f()){
		// trace("w = "<<actorState.m_next.w.x<<","<<actorState.m_next.w.y<<","<<actorState.m_next.w.z<<"   Angular Momentum "<<actorState.m_prev.Hi.x<<","<<
		// 	actorState.m_prev.Hi.y<<","<<actorState.m_prev.Hi.z<<std::endl);}

		//will distort!
	 	actorState.m_next.R = actorState.m_prev.R + actorState.getRotationDerivative(actorState.m_prev.w,actorState.m_prev.R);//  *deltaTime ;
		//another solution:
		//float angle = actorState.m_next.w.length() ;//* deltaTime;	// magnitude of w
		//vec3f axis = actorState.m_next.w;
		//axis.normalize();
		//actorState.m_next.R = actorState.m_prev.R + actorState.m_prev.RotateAngleAxis(axis,angle) * deltaTime;

		
		clearForce();

		//Gram-Schmidt
		vec3f I = vec3f(actorState.m_next.R[0],actorState.m_next.R[1],actorState.m_next.R[2]);
		vec3f J = vec3f(actorState.m_next.R[3],actorState.m_next.R[4],actorState.m_next.R[5]);
		vec3f K = vec3f(actorState.m_next.R[6],actorState.m_next.R[7],actorState.m_next.R[8]);
		vec3f e1 = I;
		vec3f e2 = J - (J * e1) / (e1 * e1) * J;
		vec3f e3 = K - (K * e1) / (e1 * e1) * K - (K * e2) / (e2 * e2) * K;


		e1.normalize();
		e2.normalize();
		e3.normalize();

		actorState.m_next.R = Matrix3f(e1[0],e2[0],e3[0],
		e1[1],e2[1],e3[1],
		e1[2],e2[2],e3[2]);
		//trace("R:"<<std::endl<<e1[0]<<","<<e2[0]<<","<<e3[0]<<std::endl<<
		//	e1[1]<<","<<e2[1]<<","<<e3[1]<<std::endl<<
		//	e1[2]<<","<<e2[2]<<","<<e3[2]<<std::endl);
		actorState.m_prev = actorState.m_next;
		
		m_position = actorState.m_next.Rcm ;





		Matrix4f translation(Matrix3f::Identity());
		translation[12] = m_position[0];
		translation[13] = m_position[1];
		translation[14] = m_position[2];
		actorMatrix = Matrix4f( actorState.m_next.R ) * translation;
		actorMatrixInv = actorMatrix.inverse();

 
	}

	NBE::vec3f Actor::getPointVelocity( vec3f ri )
	{
		return actorState.m_prev.Vcm + actorState.m_prev.w.crossProduct(ri); // v + w cross ri
	}

	vec3f Actor::solveCollision( vec3f ri, vec3f ri_other, vec3f collidingNormal, Actor& other, float restitution, bool collideThePlane )
	{
		vec3f myVel = getPointVelocity(ri);
		vec3f otherVel = other.getPointVelocity(ri_other);
		 
		float V_impulse = collidingNormal * (myVel - otherVel);

		
// 		if(V_impulse > THRESHOLD) // moving away , no collision
// 			return vec3f();
// 		if(V_impulse < -THRESHOLD) // resting contact, no collision
// 			return vec3f();

		float numerator = -(1 + restitution) * V_impulse;
		float term1 = 1 / actorState.m_prev.Mass;
		float term2 = 1 / other.actorState.m_prev.Mass;
		float term3 = collidingNormal * ((actorState.m_prev.I_inv * (ri.crossProduct(collidingNormal))).crossProduct(ri));
		float term4 = collidingNormal * ((other.actorState.m_prev.I_inv * (ri_other.crossProduct(collidingNormal))).crossProduct(ri_other));
		// Compute the impulse magnitude j
		float j = numerator / (term1 + term2 + term3 + term4);
		vec3f force = j * collidingNormal;
	 
		

	

		applyForce(ri,force);
		if(!collideThePlane) 
			other.applyForce(ri_other,-force);
		return force;
	}


	//void Camera::setChaseCam( ModelController& model )
	//{
	//	float front[] = { 0.0f, 0.0f, -20.0f };
	//	model.matEulerRotation.transform( front );
	//	float up[] = { 0.0f, 20.0f, 0.0f };
	//	model.matEulerRotation.transform( up );

	//	vec3f frontVec( front[0], front[1], front[2] );
	//	vec3f upVec( up[0], up[1], up[2] );

	//	//Lerp( m_chaseCamera.position(), m_camera.position(), 0.5f );
	//	vec3f end_m_position = model.position() - (frontVec * 5.0f ) + (upVec * 1.0f );
	//	vec3f end_m_rotation = model.rotation();

	//	m_position = Lerp( m_position, end_m_position, 0.5f );
	//	m_rotation = Lerp( m_rotation, end_m_rotation, 0.5f );

	//	//m_position = end_m_position;
	//	//m_rotation = end_m_rotation;
	//}
}